#include "nml_intf/zucpose.h"
#include "posemath.h"
#include "rtapi/rtapi_math.h"
#include <stdio.h>

//#define ZUCPOSE_PEDANTIC

void zucPoseZero(ZucPose* const pos)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pos)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    pos->tran.x = 0.0;
    pos->tran.y = 0.0;
    pos->tran.z = 0.0;
    pos->a = 0.0;
    pos->b = 0.0;
    pos->c = 0.0;
    pos->u = 0.0;
    pos->v = 0.0;
    pos->w = 0.0;
}

int zucPoseAdd(ZucPose const* const p1, ZucPose const* const p2, ZucPose* const out)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!p1 || !p2)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    pmCartCartAdd(&p1->tran, &p2->tran, &out->tran);
    out->a = p1->a + p2->a;
    out->b = p1->b + p2->b;
    out->c = p1->c + p2->c;
    out->u = p1->u + p2->u;
    out->v = p1->v + p2->v;
    out->w = p1->w + p2->w;
    return ZUCPOSE_ERR_OK;
}

int zucPoseSub(ZucPose const* const p1, ZucPose const* const p2, ZucPose* const out)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!p1 || !p2)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    pmCartCartSub(&p1->tran, &p2->tran, &out->tran);
    out->a = p1->a - p2->a;
    out->b = p1->b - p2->b;
    out->c = p1->c - p2->c;
    out->u = p1->u - p2->u;
    out->v = p1->v - p2->v;
    out->w = p1->w - p2->w;
    return ZUCPOSE_ERR_OK;
}

int zucPoseSelfAdd(ZucPose* const self, ZucPose const* const p2) { return zucPoseAdd(self, p2, self); }

int zucPoseSelfSub(ZucPose* const self, ZucPose const* const p2) { return zucPoseSub(self, p2, self); }

int zucPoseToPmCartesian(ZucPose const* const pose, PmCartesian* const xyz, PmCartesian* const abc, PmCartesian* const uvw)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
    if (!xyz | !abc || !uvw)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
#endif

    //Direct copy of translation struct for xyz
    *xyz = pose->tran;

    //Convert ABCUVW axes into 2 pairs of 3D lines
    abc->x = pose->a;
    abc->y = pose->b;
    abc->z = pose->c;

    uvw->x = pose->u;
    uvw->y = pose->v;
    uvw->z = pose->w;
    return ZUCPOSE_ERR_OK;
}

/**
 * Collect PmCartesian elements into 9D ZucPose structure.
 */
int pmCartesianToZucPose(PmCartesian const* const xyz, PmCartesian const* const abc, PmCartesian const* const uvw, ZucPose* const pose)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!xyz || !abc || !uvw)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif
    //Direct copy of translation struct for xyz
    pose->tran = *xyz;

    pose->a = abc->x;
    pose->b = abc->y;
    pose->c = abc->z;

    pose->u = uvw->x;
    pose->v = uvw->y;
    pose->w = uvw->z;
    return ZUCPOSE_ERR_OK;
}

int zucPoseSetXYZ(PmCartesian const* const xyz, ZucPose* const pose)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!xyz)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    pose->tran.x = xyz->x;
    pose->tran.y = xyz->y;
    pose->tran.z = xyz->z;
    return ZUCPOSE_ERR_OK;
}

int zucPoseSetABC(PmCartesian const* const abc, ZucPose* const pose)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!abc)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    pose->a = abc->x;
    pose->b = abc->y;
    pose->c = abc->z;
    return ZUCPOSE_ERR_OK;
}

int zucPoseSetUVW(PmCartesian const* const uvw, ZucPose* const pose)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!uvw)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    pose->u = uvw->x;
    pose->v = uvw->y;
    pose->w = uvw->z;

    return ZUCPOSE_ERR_OK;
}

int zucPoseGetXYZ(ZucPose const* const pose, PmCartesian* const xyz)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!xyz)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    xyz->x = pose->tran.x;
    xyz->y = pose->tran.y;
    xyz->z = pose->tran.z;
    return ZUCPOSE_ERR_OK;
}

int zucPoseGetABC(ZucPose const* const pose, PmCartesian* const abc)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!abc)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    abc->x = pose->a;
    abc->y = pose->b;
    abc->z = pose->c;
    return ZUCPOSE_ERR_OK;
}

int zucPoseGetUVW(ZucPose const* const pose, PmCartesian* const uvw)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
    if (!uvw)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
#endif

    uvw->x = pose->u;
    uvw->y = pose->v;
    uvw->z = pose->w;

    return ZUCPOSE_ERR_OK;
}

/**
 * Find the magnitude of an ZucPose position, treating it like a single vector.
 */
int zucPoseMagnitude(ZucPose const* const pose, double* const out)
{
#ifdef ZUCPOSE_PEDANTIC
    if (!pose)
    {
        return ZUCPOSE_ERR_INPUT_MISSING;
    }
    if (!out)
    {
        return ZUCPOSE_ERR_OUTPUT_MISSING;
    }
#endif

    double mag = 0.0;
    mag += pmSq(pose->tran.x);
    mag += pmSq(pose->tran.y);
    mag += pmSq(pose->tran.z);
    mag += pmSq(pose->a);
    mag += pmSq(pose->b);
    mag += pmSq(pose->c);
    mag += pmSq(pose->u);
    mag += pmSq(pose->v);
    mag += pmSq(pose->w);
    mag = pmSqrt(mag);

    *out = mag;
    return ZUCPOSE_ERR_OK;
}

void printZucPose(ZucPose* pose)
{
    printf(
        "x=%f\ty=%f\tz=%f\tu=%f\tv=%f\tw=%f\ta=%f\tb=%f\tc=%f", pose->tran.x, pose->tran.y, pose->tran.z, pose->u, pose->v, pose->w, pose->a, pose->b, pose->c);
}

/**
 * Return true for a numerically valid pose, or false for an invalid pose (or null pointer).
 */
int zucPoseValid(ZucPose const* const pose)
{
    if (!pose || isnan(pose->tran.x) || isnan(pose->tran.y) || isnan(pose->tran.z) || isnan(pose->a) || isnan(pose->b) || isnan(pose->c) || isnan(pose->u) ||
        isnan(pose->v) || isnan(pose->w))
    {
        return 0;
    }
    else
    {
        return 1;
    }
}

ZucPose zucPoseMult(const ZucPose& left, const ZucPose& right)
{
    PmPose l, r;
    l.tran = left.tran;
    PmRpy lrpy = {left.a, left.b, left.c};
    PmQuaternion lq;
    pmRpyQuatConvert(&lrpy, &lq);
    l.rot = lq;

    r.tran = right.tran;
    PmRpy rrpy = {right.a, right.b, right.c};
    PmQuaternion rq;
    pmRpyQuatConvert(&rrpy, &rq);
    r.rot = rq;

    PmPose ret;
    pmPosePoseMult(&l, &r, &ret);

    ZucPose out;
    out.tran = ret.tran;
    PmRpy rpy;
    pmQuatRpyConvert(&ret.rot, &rpy);
    out.a = rpy.y;
    out.b = rpy.p;
    out.c = rpy.r;
    out.u = 0.0;
    out.v = 0.0;
    out.w = 0.0;
    return out;
}